/*
 * Copyright (c) 2011-2014, fortiss GmbH.
 * Licensed under the Apache License, Version 2.0.
 *
 * Use, modification and distribution are subject to the terms specified
 * in the accompanying license file LICENSE.txt located at the root directory
 * of this software distribution. A copy is available at
 * http://chromosome.fortiss.org/.
 *
 * This file is part of CHROMOSOME.
 *
 * $Id: receiveMessageFunction.cpp 7805 2014-03-13 09:54:35Z geisinger $
 */

/**
 * \file
 *         Source file for function receiveMessage in component proxy.
 *
 * \author
 *         This file has been generated by the CHROMOSOME Modeling Tool (XMT)
 *         (fortiss GmbH).
 */

/******************************************************************************/
/***   Includes                                                             ***/
/******************************************************************************/
#include "rOSGateway/adv/proxy/include/receiveMessageFunction.h"

#include "rOSGateway/adv/proxy/include/receiveMessageFunctionWrapper.h"
#include "rOSGateway/adv/proxy/include/proxyComponent.h"
#include "rOSGateway/adv/proxy/include/proxyComponentWrapper.h"
#include "rOSGateway/adv/proxy/include/proxyManifest.h"

#include "xme/core/logUtils.h"

#include "xme/hal/include/mem.h"

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_C_INCLUDES) ENABLED START
#pragma GCC diagnostic ignored "-Wshadow" //TODO find better solution
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"

#include "xme/defines.h"
#include "xme/hal/include/ros.h" //TODO
#include "xme/hal/include/sched.h" //TODO
#include "xme/hal/include/time.h" //TODO
// PROTECTED REGION END

/******************************************************************************/
/***   Definitions                                                          ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_C_DEFINITIONS) ENABLED START
// PROTECTED REGION END

/******************************************************************************/
/***   Variables                                                            ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_C_VARIABLES) ENABLED START
static xme_hal_sched_taskHandle_t rosTaskHandle = XME_HAL_SCHED_INVALID_TASK_HANDLE;

static ros::NodeHandle* nodeHandle;
static ros::CallbackQueue callbackQueue;

static ros::Publisher message_pub;
static ros::Subscriber status_sub;
// PROTECTED REGION END

/******************************************************************************/
/***   Prototypes                                                           ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_C_PROTOTYPES) ENABLED START
static void runROSTaskCallback(void* userData);
void rOSGateway_adv_proxy_receivedStatus(const std_msgs::Bool::ConstPtr& messageStatus);
// PROTECTED REGION END

/******************************************************************************/
/***   Implementation                                                       ***/
/******************************************************************************/
xme_status_t
rOSGateway_adv_proxy_receiveMessageFunction_init
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_INITIALIZE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);

    nodeHandle = new ros::NodeHandle();

    message_pub = nodeHandle->advertise<std_msgs::String>("message", 1000);
	ros::SubscribeOptions status_sub_options = ros::SubscribeOptions::create<std_msgs::Bool>("status", 1000, rOSGateway_adv_proxy_receivedStatus, ros::VoidPtr(), &callbackQueue);
	status_sub = nodeHandle->subscribe(status_sub_options);

	rosTaskHandle = xme_hal_sched_addTask(xme_hal_time_timeIntervalFromMilliseconds(0), xme_hal_time_timeIntervalFromMilliseconds(50), 0, runROSTaskCallback, NULL);
	if (XME_HAL_SCHED_INVALID_TASK_HANDLE == rosTaskHandle)
	{
		XME_EXIT(EXIT_FAILURE, "Error creating runROSThread\n");
	}

    return XME_STATUS_SUCCESS;
    // PROTECTED REGION END
}

void
rOSGateway_adv_proxy_receiveMessageFunction_step
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    xme_status_t status[1];
    
    ROSGateway_topic_message_t portMessageData; // Required port.
    
    (void)xme_hal_mem_set(&portMessageData, 0u, sizeof(ROSGateway_topic_message_t));
    
    status[0] = rOSGateway_adv_proxy_proxyComponentWrapper_readPortMessage(&portMessageData);
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_STEP_C) ENABLED START
    	std_msgs::String msg;

        XME_UNUSED_PARAMETER(componentConfig);
        XME_UNUSED_PARAMETER(status);

        xme_fallback_printf("Proxy: receiveMessage\n");

		msg.data = portMessageData.text;
		message_pub.publish(msg);
        // PROTECTED REGION END
    }
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_STEP_2_C) ENABLED START
        // PROTECTED REGION END
    }
}

void
rOSGateway_adv_proxy_receiveMessageFunction_fini
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_TERMINATE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);
        
	delete nodeHandle;
	nodeHandle = NULL;
    // PROTECTED REGION END
}

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_RECEIVEMESSAGEFUNCTION_IMPLEMENTATION_C) ENABLED START
static void runROSTaskCallback(void* userData)
{
	XME_UNUSED_PARAMETER(userData);

	if (xme_hal_ros_ok())
	{
		callbackQueue.callAvailable(ros::WallDuration(0.01));
	}
	else
	{
		XME_EXIT(EXIT_FAILURE, "ROS has been shut-down!\n");
	}
}

void
rOSGateway_adv_proxy_receivedStatus(const std_msgs::Bool::ConstPtr& messageStatus)
{
	xme_status_t status;
	ROSGateway_topic_status_t portReceivedMessageData;
 	ROSGateway_topic_status_t* portReceivedMessageDataPtr = &portReceivedMessageData;
    
	portReceivedMessageDataPtr->value = messageStatus->data;

    xme_fallback_printf("Proxy: Received status\n");
    status = rOSGateway_adv_proxy_proxyComponentWrapper_writePortReceivedMessage(portReceivedMessageDataPtr);
    if (XME_STATUS_SUCCESS != status)
    {
        XME_LOG (XME_LOG_ALWAYS, "Write of received message status to port failed\n");
    }

    rOSGateway_adv_proxy_proxyComponentWrapper_completeWriteOperations();
}
// PROTECTED REGION END
